/**
 * @file aim_task.c/h
 * @author sunny quan
 * @brief 云台控制任务线程
 * @version 0.1
 * @date 2023-03-25
 *
 * @copyright Copyright (c) 2023 SPR
 *
 */
 /* 接线说明 */
 //PI6接下限位开关，PI7接上限位开关
 //上弹电机为load_2006_motor，ID5
 //俯仰电机gimbal_pitch_motor，ID6
 //航向电机gimbal_yaw_motor，ID待定
 //电机均挂在can2上
 
#include "aim_task.h"
#include "surgewheel_task.h"
#include "cmsis_os.h"
#include "main.h"
#include "gimbal_behaviour.h"
#include "INS_task.h"
#include "arm_math.h"
#include "tim.h"
#include "CAN_receive.h"

#define rc_deadband_limit(input, output, dealine)    \
  {                                                  \
    if ((input) > (dealine) || (input) < -(dealine)) \
    {                                                \
      (output) = (input);                            \
    }                                                \
    else                                             \
    {                                                \
      (output) = 0;                                  \
    }                                                \
  }
  
  #define gimbal_total_pid_clear(gimbal_clear)                                               \
  {                                                                                        \
    gimbal_PID_clear((gimbal_clear)->gimbal_yaw_motor.gimbal_motor_absolute_angle_pid);   \
    gimbal_PID_clear((gimbal_clear)->gimbal_yaw_motor.gimbal_motor_relative_angle_pid);   \
    PID_clear((gimbal_clear)->gimbal_yaw_motor.gimbal_motor_gyro_pid);                    \
                                                                                           \
    gimbal_PID_clear((gimbal_clear)->gimbal_pitch_motor.gimbal_motor_absolute_angle_pid); \
    gimbal_PID_clear((gimbal_clear)->gimbal_pitch_motor.gimbal_motor_relative_angle_pid); \
    PID_clear((gimbal_clear)->gimbal_pitch_motor.gimbal_motor_gyro_pid);                  \
  }

/* flag */
uint8_t load_motor_init_flag;		//上弹电机初始化flag
uint8_t up_limit_flag ;				//上限位开关flag，该flag为1时立即停止上弹电机和摩擦轮
  
/* PV */
gimbal_control_t aiming;				//云台的控制结构体
extern gimbal_behaviour_e gimbal_behaviour ;
  
extern void aim_task(void const *pvParameters)
{
	
	vTaskDelay(GIMBAL_TASK_INIT_TIME);	
	aiming_init( &aiming );
	
	while(1)
	{
		if( !load_motor_init_flag )
		{
			load_init( &aiming );
		}
		else
		{
			/* 云台功能，根据目标值对三个电机进行控制 */
			
			//设置云台控制模式
			gimbal_set_mode( &aiming );
			//云台数据反馈
			aiming_feedback_update();
			//设置云台控制量
			gimbal_set_control( &aiming );
			//云台控制PID计算
//			gimbal_control_loop();
			
			/* pid计算 */
			aiming.load_2006_motor.current_set = PID_calc(&aiming.load_2006_motor.gimbal_motor_angle_pid, aiming.load_2006_motor.absolute_angle, aiming.load_2006_motor.absolute_angle_set);
			
			if(!up_limit_flag)
			{
				/* 防堵转 */
				CAN_cmd_aim( aiming.load_2006_motor.current_set, 0, 0, 0);
			}else
			{
				/* 方向判断 */
				if( aiming.load_2006_motor.current_set < 0 )
				{
					CAN_cmd_aim( aiming.load_2006_motor.current_set, 0, 0, 0);
				}else
				CAN_cmd_aim( 0, 0, 0, 0);
				
				/* 限位开关判断 */
				if( !HAL_GPIO_ReadPin( GPIOI , GPIO_PIN_7 ) )
				{
					up_limit_flag = 0;
					
				}
			}
			
		}
		vTaskDelay(AIM_CONTROL_TIME_MS);
	}
	

}

/**
  * @brief  This function handles EXTI interrupt request.//每次使用cubemx更新后注释掉原来在hal_gpio.c里的那个
  * @param  GPIO_Pin Specifies the pins connected EXTI line
  * @retval None
  */
void HAL_GPIO_EXTI_IRQHandler(uint16_t GPIO_Pin)
{
  /* EXTI line interrupt detected */
  if(__HAL_GPIO_EXTI_GET_IT(GPIO_Pin) != RESET)
  {
    __HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
	
	  
	  switch( GPIO_Pin )
	  {
			case GPIO_PIN_6:
			{
				/* 触碰下限位 */
				load_motor_set();
				break;
			}
			
			case GPIO_PIN_7:
			{
				/* 触碰上限位 */
				up_limit_flag = 1;
				break;
			}
			
			default:
			{
				/* 无 */
				break;
			}
			
	  }

    HAL_GPIO_EXTI_Callback(GPIO_Pin);  //不要影响ins_task
  }
}


/* 瞄准线程的初始化，设置遥控器指针等内容 */
static void aiming_init( gimbal_control_t *gimbal_control )
{
	//遥控器指针获取
	gimbal_control->gimbal_rc_ctrl = get_remote_control_point();
	
	//电机数据指针获取
  gimbal_control->load_2006_motor.gimbal_motor_measure = get_yaw_gimbal_motor_measure_point();//ID5
  gimbal_control->gimbal_pitch_motor.gimbal_motor_measure = get_pitch_gimbal_motor_measure_point();//ID6
////初始化电机模式
//gimbal_control->gimbal_yaw_motor.gimbal_motor_mode =	 gimbal_control->gimbal_yaw_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
//gimbal_control->gimbal_pitch_motor.gimbal_motor_mode = gimbal_control->gimbal_pitch_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
//  //初始化云台中值与限位，实车调试后可以启用看看
//  gimbal_control.gimbal_yaw_motor.offset_ecd = GIMBAL_YAW_OFFSET_ECD;
//  gimbal_control.gimbal_yaw_motor.max_relative_angle = GIMBAL_YAW_MAX_ECD;
//  gimbal_control.gimbal_yaw_motor.min_relative_angle = GIMBAL_YAW_MIN_ECD;

	//pitch轴用的2006，这个没必要设置，考虑用陀螺仪闭环控制
//  gimbal_control.gimbal_pitch_motor.offset_ecd = GIMBAL_PITCH_OFFSET_ECD;			
//  gimbal_control.gimbal_pitch_motor.max_relative_angle = GIMBAL_PITCH_MAX_ECD;
//  gimbal_control.gimbal_pitch_motor.min_relative_angle = GIMBAL_PITCH_MIN_ECD;

  //初始化yaw电机pid
  gimbal_PID_init(&gimbal_control->gimbal_yaw_motor.gimbal_motor_absolute_angle_pid, YAW_ABSOLUTE_ANGLE_KP, YAW_ABSOLUTE_ANGLE_KI, YAW_ABSOLUTE_ANGLE_KD, YAW_ABSOLUTE_ANGLE_MAX_OUT, YAW_ABSOLUTE_ANGLE_MAX_IOUT); //陀螺仪角度控制
  PID_init(&gimbal_control->gimbal_yaw_motor.gimbal_motor_gyro_pid, YAW_GYRO_KP, YAW_GYRO_KI, YAW_GYRO_KD, YAW_GYRO_MAX_OUT, YAW_GYRO_MAX_IOUT);
  //初始化pitch电机pid
  gimbal_PID_init(&gimbal_control->gimbal_pitch_motor.gimbal_motor_absolute_angle_pid, PITCH_ABSOLUTE_ANGLE_KP, PITCH_ABSOLUTE_ANGLE_KI, PITCH_ABSOLUTE_ANGLE_KD, PITCH_ABSOLUTE_ANGLE_MAX_OUT, PITCH_ABSOLUTE_ANGLE_MAX_IOUT); //陀螺仪角度控制
  PID_init(&gimbal_control->gimbal_pitch_motor.gimbal_motor_gyro_pid, PITCH_GYRO_KP, PITCH_GYRO_KI, PITCH_GYRO_KD, PITCH_GYRO_MAX_OUT, PITCH_GYRO_MAX_IOUT);

  //初始化上弹电机pid
//  gimbal_PID_init(&gimbal_control->load_2006_motor.gimbal_motor_absolute_angle_pid, load_2006_motor_KP, load_2006_motor_KI, load_2006_motor_KP, load_2006_motor_MAX_OUT, load_2006_motor_MAX_IOUT); //陀螺仪角度控制
//  PID_init(&gimbal_control->load_2006_motor.gimbal_motor_gyro_pid, load_2006_motor_KP, load_2006_motor_KI, load_2006_motor_KP, load_2006_motor_MAX_OUT, load_2006_motor_MAX_IOUT);
  PID_init(&gimbal_control->load_2006_motor.gimbal_motor_angle_pid, load_2006_motor_KP, load_2006_motor_KI, load_2006_motor_KD, load_2006_motor_MAX_OUT, load_2006_motor_MAX_IOUT);
  PID_init(&gimbal_control->load_2006_motor.gimbal_motor_speed_pid, 10, 0, 0, 4000, 0);

//  //清除所有PID输出
//  gimbal_total_pid_clear(gimbal_control);
//  //数据更新
//  gimbal_feedback_update();
  //初始化启动状态
  gimbal_control->gimbal_yaw_motor.absolute_angle_set = gimbal_control->gimbal_yaw_motor.absolute_angle;
  gimbal_control->gimbal_yaw_motor.relative_angle_set = gimbal_control->gimbal_yaw_motor.relative_angle;
  gimbal_control->gimbal_yaw_motor.motor_gyro_set = 	gimbal_control->gimbal_yaw_motor.motor_gyro;

  gimbal_control->gimbal_pitch_motor.absolute_angle_set = 	gimbal_control->gimbal_pitch_motor.absolute_angle;
  gimbal_control->gimbal_pitch_motor.relative_angle_set = 	gimbal_control->gimbal_pitch_motor.relative_angle;
  gimbal_control->gimbal_pitch_motor.motor_gyro_set 	=	gimbal_control->gimbal_pitch_motor.motor_gyro;

  //初始化发送数据指针
//  user_send_data.gimbal_pitch_angle = &gimbal_control.gimbal_pitch_motor.absolute_angle;
//  user_send_data.gimbal_yaw_gyro = &gimbal_control.gimbal_yaw_motor.motor_gyro;

}

//控制上弹电机电流给负，直到触碰到限位开关
static void load_init( gimbal_control_t *gimbal_control )
{
	/* 上弹电机为ID5，给motor1赋电流值 */
	/* 由于给电流会加速，加入了速度闭环 */
	aiming_feedback_update();
	gimbal_control->load_2006_motor.current_set = PID_calc( &gimbal_control->load_2006_motor.gimbal_motor_speed_pid , gimbal_control->load_2006_motor.motor_speed , -8000 ) ;
	CAN_cmd_aim( gimbal_control->load_2006_motor.current_set, 0, 0, 0);
	
}

/* 设置上弹电机的初始位置 */
static void load_motor_set(void)
{
	/* 确认首次触碰限位开关时置1 */
	if( !load_motor_init_flag )
	{
		load_motor_init_flag = 1;
		aiming.load_2006_motor.absolute_angle = 0;
		load_motor_angle_reset();
		aiming.load_2006_motor.absolute_angle_set = load_motor_angle_set_after_init;
		
	}

}


/**
 * @brief 云台PID初始化函数
 *
 * @param pid PID数据结构指针
 * @param kp
 * @param ki
 * @param kd
 * @param maxout 最大输出
 * @param max_iout 最大积分输出
 */
static void gimbal_PID_init(gimbal_PID_t *pid, float kp, float ki, float kd, float maxout, float max_iout)
{
  if (pid == NULL)
  {
    return;
  }
  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;

  pid->err = 0.0f;
  pid->get = 0.0f;

  pid->max_iout = max_iout;
  pid->max_out = maxout;
}

/**
 * @brief          云台PID清除，清除pid的out,iout
 * @param[out]     gimbal_pid_clear:"gimbal_control"变量指针.
 * @retval         none
 */
static void gimbal_PID_clear(gimbal_PID_t *gimbal_pid_clear)
{
  if (gimbal_pid_clear == NULL)
  {
    return;
  }
  gimbal_pid_clear->err = gimbal_pid_clear->set = gimbal_pid_clear->get = 0.0f;
  gimbal_pid_clear->out = gimbal_pid_clear->Pout = gimbal_pid_clear->Iout = gimbal_pid_clear->Dout = 0.0f;
}

/**
 * @brief          设置云台控制模式，主要在'gimbal_behaviour_mode_set'函数中改变
 * @retval         none
 */
static void aim_set_mode(void)
{
  gimbal_behaviour_mode_set(&aiming);
}

/**
 * @brief          设置云台控制设定值，控制值是通过gimbal_behaviour_control_set函数设置的
 * @retval         none
 */
// int x;
static void gimbal_set_control( gimbal_control_t *gimbal_control )
{
  static float add_yaw_angle = 0.0f;
  static float add_pitch_angle = 0.0f;
  static float auto_yaw_target = 0.0f;
  static float auto_pitch_target = 0.0f;

	if( gimbal_behaviour == aim_load_motor_stay ) return;
	else if( gimbal_control->gimbal_rc_mode == aiming_load_motor_run ) 
	{
		gimbal_control->load_2006_motor.absolute_angle_set += gimbal_control->gimbal_rc_ctrl->rc.ch[3] * 5;
	
	}else if( gimbal_control->gimbal_rc_mode == aiming_load_motor_reset ) 
	{
		gimbal_control->load_2006_motor.absolute_angle_set = load_motor_angle_set_after_init;
	
	}
	
//  if (gimbal_control->gimbal_rc_mode == GIMBAL_AUTO)
//  {
//    if (toe_is_error(USER_USART_DATA_TOE))
//    {
//      auto_yaw_target = gimbal_control.gimbal_yaw_motor.absolute_angle;
//      auto_pitch_target = gimbal_control.gimbal_pitch_motor.absolute_angle;
//    }
//    else
//    {
//      gimbal_behaviour_control_set(&auto_yaw_target, &auto_pitch_target, &gimbal_control);
//      gimbal_control.gimbal_yaw_motor.absolute_angle_set = auto_yaw_target;
//      gimbal_control.gimbal_pitch_motor.absolute_angle_set = auto_pitch_target;
//      gimbal_absolute_angle_limit(&gimbal_control.gimbal_pitch_motor, 0.0f); //绝对角度控制限幅
//    }
//  }
//  else
//  {
//    auto_yaw_target = gimbal_control.gimbal_yaw_motor.absolute_angle;
//    auto_pitch_target = gimbal_control.gimbal_pitch_motor.absolute_angle;
//    gimbal_behaviour_control_set(&add_yaw_angle, &add_pitch_angle, &gimbal_control);
//    gimbal_control.gimbal_yaw_motor.absolute_angle_set = gimbal_control.gimbal_yaw_motor.absolute_angle_set + add_yaw_angle;
//    gimbal_absolute_angle_limit(&gimbal_control.gimbal_pitch_motor, add_pitch_angle); //绝对角度控制限幅
//  }
}

static void aiming_feedback_update(void)
{
	/* 上弹电机累计角度解算已经在can_receive中完成 */
	aiming.load_2006_motor.absolute_angle = aiming.load_2006_motor.gimbal_motor_measure->angle;
	aiming.load_2006_motor.given_current = aiming.load_2006_motor.gimbal_motor_measure->given_current;
	aiming.load_2006_motor.motor_speed = aiming.load_2006_motor.gimbal_motor_measure->speed_rpm;
	
}

void gimbal_set_mode(gimbal_control_t *gimbal_mode_set)
{
  if (gimbal_mode_set == NULL)
  {
    return;
  }

//  gimbal_behaviour_mode_set( gimbal_mode_set );
  
  /* 左上 */
  if (switch_is_up(gimbal_mode_set->gimbal_rc_ctrl->rc.s[CHASSIS_LEFT_MODE_CHANNEL]))
  {
    gimbal_mode_set->gimbal_rc_mode = aiming_load_motor_run;
  }
  /* 左中 */
  else if(switch_is_mid(gimbal_mode_set->gimbal_rc_ctrl->rc.s[CHASSIS_LEFT_MODE_CHANNEL]))
  {
	  gimbal_mode_set->gimbal_rc_mode = aiming_load_motor_stay;
  }
  /* 左下 */
  else if(switch_is_down(gimbal_mode_set->gimbal_rc_ctrl->rc.s[CHASSIS_LEFT_MODE_CHANNEL]))
  {
	  gimbal_mode_set->gimbal_rc_mode = aiming_load_motor_reset;
  }
}

